/*
 * Imu.c
 *
 *  Created on: Mar 11, 2023
 *      Author: qzl
 */

#include "filter.h"
#include "icm20602.h"
#include "math.h"
float gyro_x,gyro_y,gyro_z;
float acc_x,acc_y,acc_z;
float raw_gyro_x,raw_gyro_y;
float raw_acc_x,raw_acc_y,raw_acc_z;
float Angle_x,Angle_y,Angle_z;

/*Q值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；
但是太小则容易发散，如果Q为零，那么我们只相信预测值；
Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；
如果Q值无穷大，那么我们只信任测量值；

R值为测量噪声，太小太大都不一定合适。
R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；
越小系统收敛越快，但过小则容易出现震荡；
测试时可以保持陀螺仪不动，记录一段时间内陀螺仪的输出数据，
这个数据近似正态分布，按3σ原则，取正态分布的(3σ)^2作为R的初始化值。

测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。

系统中还有一个关键值P，它是误差协方差初始值，表示我们对当前预测状态的信任度，
它越小说明我们越相信当前预测状态；
它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。
随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，
这个时候的卡尔曼增益也是最优的，所以这个值只是影响初始收敛速度。*/
float Kalman_Filter_x(float Ang, float Gyro)
{
    static float angle;
    static float dt = 0.005f;
    static float Q_angle = 0.1f;
    static float Q_gyro = 0.0003f;
    static float R_angle = 0.1;
    char  C_0 = 1;
    static float Q_bias, Angle_err;
    static float PCt_0, PCt_1, E;
    static float K_0, K_1, t_0, t_1;
    static float Pdot[4] = {0, 0, 0, 0};
    static float PP[2][2] = { { 1, 0 }, { 0, 1 } };
    angle += (Gyro - Q_bias) * dt;
    Pdot[0] = Q_angle - PP[0][1] - PP[1][0];

    Pdot[1] = -PP[1][1];
    Pdot[2] = -PP[1][1];
    Pdot[3] = Q_gyro;
    PP[0][0] += Pdot[0] * dt;
    PP[0][1] += Pdot[1] * dt;
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;

    Angle_err = Ang - angle;

    PCt_0 = C_0 * PP[0][0];
    PCt_1 = C_0 * PP[1][0];

    E = R_angle + C_0 * PCt_0;

    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;

    t_0 = PCt_0;
    t_1 = C_0 * PP[0][1];

    PP[0][0] -= K_0 * t_0;
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;

    angle   += K_0 * Angle_err;
    Q_bias  += K_1 * Angle_err;
    return angle;
}

float Kalman_Filter_y(float Ang, float Gyro)
{
    static float angle;
    static float dt = 0.005f;
    static float Q_angle = 0.1f;
    static float Q_gyro = 0.0003f;
    static float R_angle = 0.1;
    char  C_0 = 1;
    static float Q_bias, Angle_err;
    static float PCt_0, PCt_1, E;
    static float K_0, K_1, t_0, t_1;
    static float Pdot[4] = {0, 0, 0, 0};
    static float PP[2][2] = { { 1, 0 }, { 0, 1 } };
    angle += (Gyro - Q_bias) * dt;
    Pdot[0] = Q_angle - PP[0][1] - PP[1][0];

    Pdot[1] = -PP[1][1];
    Pdot[2] = -PP[1][1];
    Pdot[3] = Q_gyro;
    PP[0][0] += Pdot[0] * dt;
    PP[0][1] += Pdot[1] * dt;
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;

    Angle_err = Ang - angle;

    PCt_0 = C_0 * PP[0][0];
    PCt_1 = C_0 * PP[1][0];

    E = R_angle + C_0 * PCt_0;

    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;

    t_0 = PCt_0;
    t_1 = C_0 * PP[0][1];

    PP[0][0] -= K_0 * t_0;
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;

    angle   += K_0 * Angle_err;
    Q_bias  += K_1 * Angle_err;
    return angle;
}



void get_angle(float* x,float* y)
{
    imu963ra_get_acc();
    imu963ra_get_gyro();
    raw_acc_x = imu963ra_acc_transition(imu963ra_acc_x);
    raw_acc_y = imu963ra_acc_transition(imu963ra_acc_y);
    raw_acc_z = imu963ra_acc_transition(imu963ra_acc_z);
	
	raw_gyro_x = imu963ra_gyro_transition(imu963ra_gyro_x);
	raw_gyro_y = imu963ra_gyro_transition(imu963ra_gyro_y);


    Angle_x = atan2f(raw_acc_y, raw_acc_z) * (-57.29577951f);
    *x = Kalman_Filter_x(Angle_x, raw_gyro_x);
	
	Angle_y = atan2f(raw_acc_x, raw_acc_z) * (-57.29577951f);
    *y = Kalman_Filter_y(Angle_y, raw_gyro_y);
}
